﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Emgu.CV;
using Emgu.CV.UI;
using Emgu.CV.Structure;

namespace VisionTestApp2
{
    public class PyramidMatch
    {
        public static EstimatedPose FindPose(Image<Gray, Byte> source, Image<Gray, Byte> template)
        {
            var size = 2* Math.Max(template.Width, template.Height);
            var clipX = (int) Math.Max((float)(source.Width-size)/2.0f,0f);
            int clipY = (int) Math.Max((float)(source.Height-size) / 2.0f, 0f);
            var clipRectangle = new System.Drawing.Rectangle(Math.Min(clipX, source.Width), Math.Min(clipY, source.Height), Math.Min(size, source.Width), Math.Min(size,source.Height));
            

            source = source.GetSubRect(clipRectangle);

            // figure out max levels
            int smallestTemplateDimension = Math.Min(template.Width, template.Height);

            // template is always smaller then image, so figure out # levels needed to make it 10 pix across
            int level = 1;
            while (smallestTemplateDimension>10)
            {
                smallestTemplateDimension /= 2;
                level++;
            }

            // build pyramids
            Image<Gray, Byte>[] sourcePyramid = source.BuildPyramid(level);
            Image<Gray, Byte>[] templatePyramid = template.BuildPyramid(level);

            
            float startAngle = 0;
            float endAngle = 360;
            const int step = 20;
            List<Template> imageSet;
            EstimatedPose result = new EstimatedPose();

            float[] delta = {0.1f,0.5f,15f,30f,180f,180f,180f,180f};

            while (level >= 0)
            {
                startAngle = result.Theta - delta[level];
                endAngle = result.Theta + delta[level];
                imageSet = BuildRotatedTemplateSet(templatePyramid[level], startAngle, endAngle, step);
                result = FindBestMatch(imageSet, sourcePyramid[level]);
                level--;
            }

            // correct to positive high score
            result.Score = 1.0 - result.Score;

            // correct clipped x,y coords to original image coords
            result.X += clipRectangle.X;
            result.Y += clipRectangle.Y;

            return result;
        }

        private static EstimatedPose FindBestMatch(List<Template> templates, Image<Gray, byte> image)
        {
            List<System.Drawing.Point> Points = new List<System.Drawing.Point>();
            List<double> Scores = new List<double>();
            

            foreach (var template in templates)
            {
                var result = image.MatchTemplate(template.Image, Emgu.CV.CvEnum.TM_TYPE.CV_TM_SQDIFF_NORMED);
                double[] minValues;
                double[] maxValues;
                System.Drawing.Point[] minPoints;
                System.Drawing.Point[] maxPoints;
                result.MinMax(out minValues, out maxValues, out minPoints, out maxPoints);
                Points.Add(minPoints[0]);
                Scores.Add(minValues[0]);
            }

            int lowIndex = 0;
            double lowValue = 1;

            for (int i = 0; i < Points.Count; i++)
            {
                if (lowValue > Scores[i])
                {
                    lowValue = Scores[i];
                    lowIndex = i;
                }
            }

            var returnValue = new EstimatedPose();
            returnValue.X = Points[lowIndex].X;
            returnValue.Y = Points[lowIndex].Y;
            returnValue.Theta = templates[lowIndex].Angle;
            returnValue.Score = Scores[lowIndex];

            return returnValue;
        }

        private static List<Template> BuildRotatedTemplateSet(Image<Gray, Byte> template, float startAngle, float endAngle, int steps)
        {
            List<Template> results = new List<Template>();
            float step = (endAngle - startAngle) / steps;
            for (float angle = startAngle; angle < endAngle; angle += step)
            {
                var transform = new RotationMatrix2D<double>(new System.Drawing.PointF(template.Width / 2, template.Height / 2), angle, 1);
                results.Add(new Template() { Image = template.WarpAffine(transform, Emgu.CV.CvEnum.INTER.CV_INTER_AREA, Emgu.CV.CvEnum.WARP.CV_WARP_FILL_OUTLIERS, new Gray(0)), Angle = angle });
            }

            return results;
        }

        private class Template
        {
            public Image<Gray, Byte> Image;
            public float Angle;
        }
    }
}
